#include "HeightMapCoordinates.h"
#include "HeightMap.h"
#include "SharedConstants.h"
#include "Ground.h"

std::pair<size_t, size_t> HeightMapUtils::FindHeightMapIndexes(const TVector2& i_point,
                                                               const TVector2& i_origin_xy, double i_cell_size)
  {
  size_t x_dimention = (size_t)((i_point.X() - i_origin_xy.X()) / i_cell_size);
  size_t y_dimention = (size_t)((i_point.Y() - i_origin_xy.Y()) / i_cell_size);

  return std::make_pair(x_dimention, y_dimention);
  }

std::pair<size_t, size_t> HeightMapUtils::FindHeightMapIndexes(const TVector2& i_point)
  {
  return FindHeightMapIndexes(i_point, TVector2(GroundConstants::ORIGIN_X, GroundConstants::ORIGIN_Y),
    GroundConstants::CELL_SIZE);
  }
TVector3 HeightMapUtils::ConvertHMapPointToTVector(size_t i_x, size_t i_y, double i_height,
                                                        const TVector2 &i_origin_xy, double i_cell_size)
  {
  return TVector3(i_x * i_cell_size + i_origin_xy.X(), i_y * i_cell_size + i_origin_xy.Y(), i_height);
  }

TVector3 HeightMapUtils::ConvertHMapPointToTVector(size_t i_x, size_t i_z, double i_height)
  {
  return ConvertHMapPointToTVector(i_x, i_z, i_height, TVector2(GroundConstants::ORIGIN_X, GroundConstants::ORIGIN_Z),
                                   GroundConstants::CELL_SIZE);
  }

TVector3 HeightMapUtils::TransformPositionToCenterOfCell(TVector3 i_position)
  {
  auto cell = HeightMapUtils::FindHeightMapIndexes(TVector2(i_position.X(), i_position.Y()));
  return TVector3((cell.first + 0.5) * GroundConstants::CELL_SIZE, (cell.second + 0.5) * GroundConstants::CELL_SIZE, i_position.Z());
  }

bool HeightMapUtils::CellBelowTriangle(size_t i_x, size_t i_y, const TTriangle2 i_triangle)
  {
  TVector2 top_left_point = TVector2(i_x * GroundConstants::CELL_SIZE, i_y * GroundConstants::CELL_SIZE);
  TVector2 top_right_point = TVector2((i_x + 1) * GroundConstants::CELL_SIZE, i_y * GroundConstants::CELL_SIZE );
  TVector2 bottom_left_point = TVector2(i_x  * GroundConstants::CELL_SIZE, (i_y + 1) * GroundConstants::CELL_SIZE );
  TVector2 bottom_right_point = TVector2((i_x + 1)  * GroundConstants::CELL_SIZE, (i_y + 1) * GroundConstants::CELL_SIZE );
  
  TTriangle2 top_triangle = TTriangle2(top_left_point, top_right_point, bottom_left_point);
  TTriangle2 bottom_triangle = TTriangle2(bottom_right_point, bottom_left_point, top_right_point);
  
  return (AreaIntersectionTest(top_triangle, i_triangle) || AreaIntersectionTest(bottom_triangle, i_triangle));
  }

void HeightMapUtils::GetObjectPlacementCoords( TVector3& o_coords, const Ground& i_ground, const TVector3& i_world_point)
  {
  double height = i_ground.GetHeightForXY(TVector2(i_world_point.X(), i_world_point.Y()));
  TVector3 point(i_world_point.X(), i_world_point.Y(), height * GroundConstants::CELL_SIZE);
  o_coords = HeightMapUtils::TransformPositionToCenterOfCell(point);
  }